package com.rockwell.robot2015.model;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;

public class ColorBlobDetector {
	// Lower and Upper bounds for range checking in HSV color space
	private Scalar mLowerBound = new Scalar(0);
	private Scalar mUpperBound = new Scalar(0);
	// Minimum contour area in percent for contours filtering
	private static double mMinContourArea = 0.5;
	
	// Color radius for range checking in HSV color space
	// private Scalar mColorRadius = new Scalar(25,50,50,0);
	
	private Scalar mColorRadius = new Scalar(12, 35, 70, 0);
	
	private Scalar mColorLine = new Scalar(255, 255, 255);
	private Scalar mColorCenter = new Scalar(0, 0, 255);
	
	private Mat mSpectrum = new Mat();
	private List<MatOfPoint> mContours = new ArrayList<MatOfPoint>();

	private Scalar m_contourColor = new Scalar(0);

	// Cache
	Mat mPyrDownMat = new Mat();
	Mat mHsvMat = new Mat();
	Mat mMask = new Mat();
	Mat mDilatedMask = new Mat();
	Mat mHierarchy = new Mat();

	public ColorBlobDetector(Scalar contourColor) {
		m_contourColor = contourColor;
	}

	public void setColorRadius(Scalar radius) {
		mColorRadius = radius;
	}

	public void setHsvColor(Scalar hsvColor) {
		double minH = (hsvColor.val[0] >= mColorRadius.val[0]) 
				? hsvColor.val[0] - mColorRadius.val[0]
				: 0;
				
		double maxH = (hsvColor.val[0] + mColorRadius.val[0] <= 255) 
				? hsvColor.val[0] + mColorRadius.val[0]
				: 255;

		mLowerBound.val[0] = minH;
		mUpperBound.val[0] = maxH;

		mLowerBound.val[1] = hsvColor.val[1] - mColorRadius.val[1];
		mUpperBound.val[1] = hsvColor.val[1] + mColorRadius.val[1];

		mLowerBound.val[2] = hsvColor.val[2] - mColorRadius.val[2];
		mUpperBound.val[2] = hsvColor.val[2] + mColorRadius.val[2];

		mLowerBound.val[3] = 0;
		mUpperBound.val[3] = 255;

		Mat spectrumHsv = new Mat(1, (int) (maxH - minH), CvType.CV_8UC3);

		for (int j = 0; j < maxH - minH; j++) {
			byte[] tmp = { (byte) (minH + j), (byte) 255, (byte) 255 };
			spectrumHsv.put(0, j, tmp);
		}

		Imgproc.cvtColor(spectrumHsv, mSpectrum, Imgproc.COLOR_HSV2RGB_FULL, 4);
	}

	public Mat getSpectrum() {
		return mSpectrum;
	}

	public void setMinContourArea(double area) {
		mMinContourArea = area;
	}

	public void process(Mat rgbaImage) {
		Imgproc.pyrDown(rgbaImage, mPyrDownMat);
		Imgproc.pyrDown(mPyrDownMat, mPyrDownMat);

		Imgproc.cvtColor(mPyrDownMat, mHsvMat, Imgproc.COLOR_RGB2HSV_FULL);

		Core.inRange(mHsvMat, mLowerBound, mUpperBound, mMask);
		Imgproc.dilate(mMask, mDilatedMask, new Mat());

		List<MatOfPoint> contours = new ArrayList<MatOfPoint>();

		Imgproc.findContours(mDilatedMask, contours, mHierarchy,
				Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);

		// Find max contour area
		double maxArea = 0;
		Iterator<MatOfPoint> each = contours.iterator();
		
		while (each.hasNext()) {
			MatOfPoint wrapper = each.next();
			double area = Imgproc.contourArea(wrapper);
			if (area > maxArea)
				maxArea = area;
		}

		// Filter contours by area and resize to fit the original image size
		mContours.clear();
		each = contours.iterator();
		while (each.hasNext()) {
			MatOfPoint contour = each.next();
			if (Imgproc.contourArea(contour) > mMinContourArea * maxArea) {
				Core.multiply(contour, new Scalar(4, 4), contour);
				mContours.add(contour);
			}
		}
	}

	public boolean detectShape(Mat rgbaImage, ColorBlobInfo info) {
		float[] r = { 0.0f };
		Point c = new Point();

		float[] curR = { 0.0f };
		Point curC = new Point();

		Rect rect = null;
		
		Iterator<MatOfPoint> each = mContours.iterator();
		while (each.hasNext()) {

			MatOfPoint contour = each.next();
			MatOfPoint2f contourf = new MatOfPoint2f(contour.toArray());

			Imgproc.minEnclosingCircle(contourf, curC, curR);

			if (curR[0] > r[0]) {
				r[0] = curR[0];
				c.x = curC.x;
				c.y = curC.y;
				
				rect = Imgproc.boundingRect(contour);
			}
		}

		if (r[0] > 0) {
			info.center.x = c.x;
			info.center.y = c.y;
			info.radius = r[0];
			info.boundingRect = rect;
			
			if (info.detectLine) {
				//detectLine(rgbaImage, info);
			}

			/*
			Core.circle(rgbaImage, c, 3, mColorCenter, 2, 8, 0);
			Core.circle(rgbaImage, c, (int) r[0], m_contourColor, 2, 8, 0);

			int width = rgbaImage.width();
			int height = rgbaImage.height();

			Core.line(rgbaImage, new Point(
					width * IDetector.FOV_WIDTH_BOUNDARY, 
					height * IDetector.FOV_HEIGHT_BOUNDARY), 
					new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
					m_contourColor, 2);

			Core.line(rgbaImage, new Point(0, height * IDetector.FOV_HEIGHT_BOUNDARY), 
					new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * IDetector.FOV_HEIGHT_BOUNDARY),
					m_contourColor, 2);

			Core.line(rgbaImage, new Point(0, rgbaImage.height() * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
					new Point(width * IDetector.FOV_WIDTH_BOUNDARY, height * (1 - IDetector.FOV_HEIGHT_BOUNDARY)), 
					m_contourColor, 2);
			*/
			/*
			double treasureCenterX = width * IDetector.TREASURE_CENTER_POSITION_X;
			double treasureCenterY = height * IDetector.TREASURE_CENTER_POSITION_Y;
			double halfTolerance = (height * IDetector.TREASURE_CENTER_TOLERANCE) / 2.0f;

			Point corner1 = new Point(treasureCenterX - halfTolerance, treasureCenterY - halfTolerance);
			Point corner2 = new Point(treasureCenterX + halfTolerance, treasureCenterY + halfTolerance);

			Core.rectangle(rgbaImage, corner1, corner2, m_contourColor, 2);
			*/
			return true;
		} else {
			return false;
		}
	}

	private void detectLine(Mat imageSource, ColorBlobInfo info) {
		info.line[0] = info.line[1] = null;
		
		Mat imageGray = new Mat();

		Imgproc.cvtColor(imageSource, imageGray, Imgproc.COLOR_RGB2GRAY, 4);
		Imgproc.Canny(imageGray, imageGray, 80, 100, 3, true);
		
		Mat lines = new Mat();
		int threshold = 50;
		int minLineSize = 20;
		int lineGap = 20;

		Imgproc.HoughLinesP(imageGray, lines, 1, Math.PI / 180, threshold,
				minLineSize, lineGap);

		int longest = -1;
		double longestLength = 0;
		
		for (int x = 0; x < lines.cols(); x++) {
			double[] vec = lines.get(0, x);
			double x1 = vec[0], y1 = vec[1], x2 = vec[2], y2 = vec[3];
			
			if (isInside(x1, y1, info.center, info.radius) && 
				isInside(x2, y2, info.center, info.radius)) {
				
				double dx = x1 - x2;
				double dy = y1 - y1;
				
				double dist = dx * dx + dy * dy;
				
				if (dist > longestLength) {
					longest = x;
					longestLength = dist;
				}
			}
		}

		if (longest > 0) {
			double[] vec = lines.get(0, longest);
			double x1 = vec[0], y1 = vec[1], x2 = vec[2], y2 = vec[3];
	
			Point start = new Point(x1, y1);
			Point end = new Point(x2, y2);
	
			info.line[0] = start;
			info.line[1] = end;
			
			Core.line(imageSource, start, end, mColorLine, 3);
		}
	}

	public List<MatOfPoint> getContours() {
		return mContours;
	}
	
	private boolean isInside(double x, double y, Point center, double radius) {
		double dx = x - center.x;
		double dy = y - center.y;
		double dist2 = dx * dx + dy * dy;
		return dist2 <= radius * radius + 0.5;
	}
}
